#note: the range scanner's degree is +90
#note: added  3.times{temprange.shift} to drawranges
#can only be used with type lines

require "gl"
require "glu"
require "glut"
require "mathn"

#Add gl and glut namespaces in order to make porting easier
include Gl
include Glu
include Glut


CONFIGFILENAME="rawmapper.cfg"
 
#default variables
$scale=3.0 #default value,normally drawn from the config file
$robotno=1
$viewcount=0
$logfilename="../../mapdata/geomap_robot#{$viewcount}.dat" #default log file name value
$type="dots" 
$delay=0.01 #sets the delay refresh rate of the Mapper to increase performance (0.2-0.3)
$runtype=0 #0 for live, 1 for log 
$scaninterval=1.0 #The laser scan interval, in one scan how many degrees

#Program variables
$start_x=0
$start_y=0
$inslogarray=[]
$fsonar=[]
$rsonar=[]
$robotrange=[]

$fsonarpos=[-90,-50,-30,-10,10,30,50,90] #the position of the fsonar on the P2AT Bot
$rsonarpos=[90,130,150,170,-170,-150,-130,-90] #the position of the rsonar on the P2AT Bot

$fsonarpara=+0.1
$fsonarypos=[0.145,0.185+$fsonarpara,0.22+$fsonarpara,0.24+$fsonarpara,0.24+$fsonarpara,0.22+$fsonarpara,0.185+$fsonarpara,0.145-0.1]
$fsonarxpos=[-0.130,-0.115+$fsonarpara,-0.080+$fsonarpara,-0.025+$fsonarpara,0.025+$fsonarpara,0.080+$fsonarpara,0.015+$fsonarpara,0.130-0.1]

$rsonarxpos=[-0.145,-0.185,-0.22,-0.24,-0.24,-0.22,-0.185,-0.145]
$rsonarypos=[0.130,0.115,0.080,0.025,-0.025,-0.080,-0.015,-0.130]
window =""
$screen=1


#OpenGl Variable 
$viewx=0 #view point starting view, default=0
$viewy=0 #view point starting view,default=0
$s_width=640
$s_height=480
$look_x=0.0
$look_y=0.0

#draw which element
$drawrs=0
$drawfs=1
$laserscan=1
$showlaserray=0

#used for multiple robots
$robotname=[]

#cumilative data
$mem_robotpos=[]
$mem_start_x=[]
$mem_start_y=[]
$mem_robotname=[]
$mem_robotins=[]

#opengl data
$mem_wall=[] # $mem_wall[robotno][[x,y],[x,y]....]


#2D View simulated by drawing only on the -6.0 z-plane and viewing only upwards
#from the -12.0 z-plane

#get the starting xy value of the robot (UNUSED)
def getstartxy()
 open($logfilename,"r"){|file|
   file.each_line { |line|  
  if (line =~ /INS,/)
  temparray=line.split(",") #reads the first line
  $start_x=temparray[2].to_f
  $start_y=temparray[3].to_f
  break 
  end
  }
 }
end


def getdirdata()
  dirlist=Dir.entries("../../mapdata")
  $mapdatalist=[]

  dirlist.each{|filename|
    # puts filename
     $mapdatalist<<filename  if (filename=~/geomap/i )
  }

  if($DEBUG==1)
  puts "-----"
  puts $mapdatalist
  end

  if($mapdatalist.size<1)
    raise "No readable files found "
 else
  $logfilename="../../mapdata/"+$mapdatalist[$viewcount]
 end


end

#This function reads the configuration file 
def readconfig()
  
   #try to read the config file (mapper.cfg)
   raise "Cannot read config file #{CONFIGFILENAME}"  unless (File.file?(CONFIGFILENAME))
   open(CONFIGFILENAME,"r"){|file|
   raise "config file corrupted" unless (file.readline()=~ /\[USARSIM_RAW_MAPPER\]/)
   
   #load the config file data
    file.each_line() { |line|      

      #set the map scale from the config file 
      if line =~ /\[MAP_SCALE=([A-Za-z.0-9]+)\]/
        mapscale=$1 
        if mapscale =~/[0-9]*\.[0-9]+|[0-9]+/  and mapscale.to_f <100 and mapscale.to_f >0
          $scale=mapscale.to_f
          puts "setting map scale data to #{$scale}"
        else
         puts "cannot read the map scale parameter, defaulting to #{$scale}, should be set to something like MAP_SCALE=3.0"
       end
      end
      #set the map type from the config file
       if line =~ /\[TYPE=([A-Za-z.0-9]+)\]/
         maptype=$1
        if maptype =~/lines|dots|line|dot/i #check if the type is lines,dots 
         $type=maptype 
          puts "setting map type to #{$type}"
        else
         puts "cannot read the map type parameter, defaulting to #{$type}, options include lines or dots"
       end
      end
      #set the update delay from the config file
        if line =~ /\[UPDATE_DELAY=([A-Za-z.0-9]+)\]/
         upde=$1
        if upde =~ /[0-9]*\.[0-9]+|[0-9]+/ #check if the number is a float
         $delay=upde.to_f
          puts "setting the update delay speed to #{$delay} seconds"
        else
         puts "cannot read the update delay parameter, defaulting to #{$type}, should be set to (0.1-0.3)"
       end
      end
     #set the robot number
      if line =~ /\[ROBOT_NO=([A-Za-z.0-9]+)\]/
         uprobno=$1
        if uprobno =~ /[1-9]/ #check if the number is an integer
         $robotno=uprobno.to_i
          puts "setting the robot number to #{$robotno} "
        else
         puts "cannot read the robot number parameter, defaulting to #{$robotno}, should be set to (1-9)"
       end
      end
    #set the map run type from the config file
       if line =~ /\[RUN_TYPE=([A-Za-z.0-9]+)\]/
         runtype=$1
        if runtype =~/live/i #check if the run type is live
         $runtype=0
         puts "setting run type to #{$runtype}"
         getdirdata()
         $mode=0
        # $logfilename="../../mapdata/geomap_robot#{$viewcount}.dat" #default log file name value for live run
        elsif runtype=~/log/i #check if the run type is log 
          $runtype=1
         puts "setting run type to #{$runtype}"
         $logfilename="rawmapper.dat" #default log file name value log run
         $mode=1
        else
         puts "cannot read the run type parameter, defaulting to #{$runtype}, options include live or log"
         
       end
      end
      
    }
   } 
 
end

#This function deals with the opengl windows settings
def init_gl_window(width ,height )

#glClearColor specifies the red, green, blue, and alpha values used by
#glClear to clear the color buffers. sets the value to be used in glClear(GL_COLOR_BUFFER_BIT)
glClearColor(0.0,0.0,0.0,0) 

glClearDepth(1.0) # enables the clearing of the depth buffer, used in glClear(GL_DEPTH_BUFFER_BIT)
glDepthFunc(GL_LEQUAL) #set type of depth
glEnable(GL_DEPTH_TEST) #enables depth test
glShadeModel(GL_SMOOTH) # smooth color shading

glMatrixMode(GL_PROJECTION) #Applies subsequent matrix operations to the projection matrix stack.
glLoadIdentity #clear old matrix
gluPerspective(45.0, width / height, 0.1, 100.0)# Calculate aspect ratio of the window 45 degrees,ratio of width/height, Znear,Zfar.
glMatrixMode(GL_MODELVIEW) #Applies subsequent matrix operations to the modelview matrix stack.
draw_gl_scene #draw scence
end

#This function deals with what happends when the windows reshapes itself
def reshape(width, height)
    height = 1 if height == 0 #prevent division by zero
    # Reset current viewpoint and perspective transformation
    # glViewport(Lowerright_x,Lowerright_y,width,height)
    $s_width=width
    $s_height=height
    glViewport($viewx, $viewy, width, height)

    glMatrixMode(GL_PROJECTION) #Applies subsequent matrix operations to the projection matrix stack.
    glLoadIdentity #Clear the Projection matrix stack 
    gluPerspective(45.0, $s_width / $s_height, 0.1, 100.0) #reset the  Perspective
end

#inputs a degree in and return the value of it's radian
def de2rad(float)
  rad=float
  pi=3.14159265
  if (rad>0)
    return ((2*pi)/360)*rad
  else
    return 2*pi+(((2*pi)/360)*rad)
  end
end


#This function draws the INS Position
def drawINS(array,startx,starty) 
 insarray=array
 lines_old_x=startx/$scale
 lines_old_y=starty/$scale
 insarray.each_with_index { |line,index| 
  temparray=line
    #draws lines 
     if ($type=~/lines|line/i and index>0)
    glBegin(GL_LINES)
      glVertex3f(lines_old_x,lines_old_y,0.0)
      glVertex3f( (temparray[3].to_f)/$scale , (temparray[4].to_f)/$scale,0.0 )
    # puts "Hi: #{temparray[3].to_f-startx}"
    glEnd
    #store the points on a variable
    lines_old_x=(temparray[3].to_f)/$scale
    lines_old_y=(temparray[4].to_f)/$scale
    end
  }
end


#output a string of text to be shown on open_gl
def output(fx,fy,fz,fstring)
  x=fx
  y=fy
  z=fz
  ostring=fstring
  i=0
  glRasterPos3f(x, y,z);
  ostring.length.times{|i|
    glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18,ostring[i] );
  }
end


def draw_map()
  
  #draw a yellow X at the start point  
  glPushMatrix()
  glColor(1.0, 1.0, 0.0)
  glTranslatef(0.05,0.0,0.0) #move the letter x to the starting poinst
  #Start center of the screen with a depth of six 
  #output(0.0,-0.03,-6.0,"x")  #default lenght of the character X
 $mem_start_x.length.times{|i| 
  output($mem_start_x[i]/$scale, ($mem_start_y[i]/$scale)-0.03 ,-6.0,"x")
 }
  glPopMatrix()
 
#draw last robot position
$mem_robotpos.length.times{|i|
  glPushMatrix()
  glColor(1.0, 0.0, 0.0)
  temparray=Array.new($mem_robotpos[i])
  # get the robot position 
  robotx=temparray[3]
  roboty=temparray[4]
  
  glTranslatef( (robotx/$scale),(roboty/$scale),-6.0)  
  quadratic=gluNewQuadric();
  gluSphere(quadratic,0.05,32,32)
  glPopMatrix()
  
  #draw robot head Note INS Pitch,Roll,Yaw data is in radians when the yaw isn't zero
 if(temparray[8].to_f!=0)
  glPushMatrix()
  glColor(1.0,1.0,1.0)
  #calculate the head angle
  headx=0.05*(Math.cos(temparray[8].to_f))
  heady=0.05*(Math.sin(temparray[8].to_f))
  #puts "headx=#{headx} heady=#{heady} robotx=#{robotx} roboty=#{roboty}"
  #glTranslatef( ((robotx/$scale)+headx),((roboty/$scale)+heady)*-1,-6.0) #inverted mode
   glTranslatef( ((robotx/$scale)+headx),((roboty/$scale)+heady),-6.0)   
  quadratic=gluNewQuadric();
  gluSphere(quadratic,0.025,32,32)
  glPopMatrix()
 end 
}

# draw robot range 
  $mem_robotins.length.times{|i| 
  glPushMatrix()
  glColor(1.0/(i+1), 1.0/(i+i), 1.0/(i+i))
  glTranslatef(0.0, 0.0, -6.0)   #Start center of the screen with a depth of six
  drawINS($mem_robotins[i],$mem_start_x[i],$mem_start_y[i])   
  glPopMatrix()
  }
  
# draws the wall
  glPushMatrix()
  glTranslatef(0.0, 0.0, -6.0)
  #puts $mem_wall[0].size if $mem_wall[0]
$mem_wall.each{|robot|
    robot.each{|wall|
    glBegin(GL_POINTS)
     glColor(1.0, 1.0,1.0)
      glVertex3f( wall[0]/$scale ,wall[1]/$scale,wall[2]/$scale)
    glEnd
    }
}  
glPopMatrix()

end



# This functions tells opengl what model to draw
def draw_gl_scene()
 
  sleep($delay)
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) #clear the color & depth buffer (clear screen)
  #resets the model,destroying any old models
  glMatrixMode(GL_MODELVIEW)
  glLoadIdentity
  #gluLookAt(eye,point,up) Picture drawn on z axis -6 viewed from -12
  gluLookAt($look_x,$look_y, -12.0, $look_x, $look_y,-6.0, 0.0, 1.0, 0.0);
  #log the data into the correct format 
  
  #clear old data 
  $inslogarray=[]
  $fsonar=[]
  $rsonar=[]
  $robotname=[]
  
  #team robots
  $robotinsarray=[]
  $robotfsonar=[]
  $robotrsonar=[] 
  $robotrange=[]
  $start_x=[]
  $start_y=[]
  robotid=0
  
  #clear mem data if log mode
  #cumilative data
  if($runtype==1)
    $mem_robotpos=[]
    $mem_start_x=[]
    $mem_start_y=[]
    $mem_robotname=[]
    $mem_robotins=[]
    $mem_wall=[] # $mem_wall[robotno][[x,y],[x,y]....]
  end
  
 #log from file  (read once)
 
  open($logfilename,"r") do |file|
   if (file)
   file.each_with_index {  |line,index|
   temparray=line.split(",") #reads the first line
   #check the robot name and match with the id position
   robotname=temparray[2]
   #match with the robotname's memory 
   if($mem_robotname.size==0)
     $mem_robotname<<robotname
     $mem_robotins=$mem_robotins<<Array.new
     $robotinsarray=$robotinsarray <<Array.new
     $mem_wall=$mem_wall<<Array.new
     robotid=$robotinsarray.length-1
   end   
   $mem_robotname.each_with_index { |member,index|
   if(robotname==member)
     robotid=index
     break
   end    
   if(index==$mem_robotname.length-1) #not found add to the robot memory
     $mem_robotname<<robotname
     $robotinsarray=$robotinsarray <<Array.new
     $mem_robotins=$mem_robotins<<Array.new
     $mem_wall=$mem_wall<<Array.new
     robotid=$robotinsarray.length-1
   end
   } 
   
   #update the INS information if the 
   if(line=~/INS,/) 
   temparray=line.split(",")
   #get the start_x and start_y values for the first starting point
   $mem_start_x[robotid]=temparray[3].to_f if !$mem_start_x[robotid]
   $mem_start_y[robotid]=temparray[4].to_f if !$mem_start_y[robotid]
    # get the robot position 
    #write the robot position   
    type=temparray[0]
    time=temparray[1].to_f
    name=temparray[2].to_f
    robotx=temparray[3].to_f
    roboty=temparray[4].to_f  
    robotz=temparray[5].to_f
    yaw=temparray[6].to_f
    pitch=temparray[7].to_f
    row=temparray[8].to_f
    pos=Array.new
    pos<<type<<time<<name<<robotx<<roboty<<robotz<<yaw<<pitch<<row
    #add last position to the robot position log
    $mem_robotpos[robotid]=Array.new(pos)
    #add the INS file
    $mem_robotins[robotid][($mem_robotins[robotid].length)]=pos
   end
   
   #RANGESCAN,67.77,robot3,1.3306,1.3349,1.3379,1.3447              
  
   #store the rangescanner information into the
   if(line=~ /RANGESCAN,/i)
     
    #calculate the wall position
    temprange=line.split(",")
    if(temprange[1].to_f>0) #ignore if time== -1
      3.times{temprange.shift}
        robotx=$mem_robotpos[robotid][3].to_f
        roboty=$mem_robotpos[robotid][4].to_f
        ang=$mem_robotpos[robotid][8].to_f
        temprange.each_with_index{|member,index|  
        rad=(0.0174*(temprange.size-index)*$scaninterval)
        dis=member.to_f
        sx=robotx
        sy=roboty
        robotang=ang-(3.1415/2)
        if(dis<19.9)
           drawx=(dis)*(Math.cos(rad+robotang))
           drawy= (dis)*(Math.sin(rad+robotang))
           wall=[]
           wall<<(sx+drawx) <<(sy+drawy)<< 0.0
           $mem_wall[robotid]<<wall
        end     
        }
    end
          
    end    
   } #all lines
   
      
  end # if(file)
   end #open file


 #if live mode
 if ($runtype==0)
   open($logfilename,"w") 
 end
  
 puts $mem_wall[0].size if $mem_wall[0]
  
 draw_map() if $screen==1
  
  #swap buffers for display
  glutSwapBuffers
  

    
end

# The idle function to handle 
def idle
    # Just redisplays the screen
    glutPostRedisplay if ($runtype==0) #update if live
end

# Keyboard handler to exit when ESC is typed
keyboard = lambda do |key, x, y|
    case(key)
      when 27
          glutDestroyWindow(window)
          exit(0)
      when ?a  # the a key, go left
          $look_x+=0.5 if $screen==1
          
      when ?d  # the d key, go right
          $look_x-=0.5 if $screen==1
          
      when ?w # the w key, go up
          $look_y+=0.5 if $screen==1
        
      when ?s # the s key, go down
          $look_y-=0.5 if $screen==1
      when ?+ 
          $scale-=1 if $scale>1 and $screen==1
      when ?-
          $scale+=1 if $scale<20 and $screen==1
      when ?h
          $screen=2  if $screen==1
      when ?m
          $screen=1 if $screen==2
      when ?n
           if $viewcount<$mapdatalist.size-1 and $mode==0
         $viewcount=$viewcount+1 
         getdirdata()
         puts "loading from #{$logfilename}"
        elsif $viewcount>=$mapdatalist.size-1 and $mode==0
         $viewcount=0
         getdirdata()
          puts "loading from #{$logfilename}"
        end
      when ?p
        if $viewcount>0  and $mode==0
           $viewcount=$viewcount-1 
         getdirdata()
         puts "loading from #{$logfilename}"
        elsif $viewcount<=0 and $mode==0
           $viewcount=$mapdatalist.size-1
           getdirdata()
           puts "loading from #{$logfilename}"
        end
   end
      glutPostRedisplay
end

####################################################
#           Start initiating our code here         #
####################################################


#gets the values from the configuration file
readconfig()
#sets the startxy of the first robot, used as the center for the screen
#getstartxy() 

#initiate our OpenGL
glutInit()
#Sets the display mode to be of double buffer(enables buffer swapping) with RGB mode
#and requests an alpha and depth buffer
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
#Sets the window size and start position
glutInitWindowSize($s_width, $s_height);
glutInitWindowPosition(0,0);

window=glutCreateWindow("USAR_SIM_MAPPER") #creates a top level window

#glutDisplayFunc sets thedisplay callback for the current window. When GLUT determines 
#that the normal plane for the window needs to be redisplayed, the display callback 
# the window is called. GLUT determines when the display callback should be triggered
#  based on the window's redisplay state.when displaying, the method draw_gl_scence is called  
glutDisplayFunc(method(:draw_gl_scene).to_proc) 


#What method to call when the window is reshaped
glutReshapeFunc(method(:reshape).to_proc);

#What method to call while looping
glutIdleFunc(method(:idle).to_proc);

#uses the method keyboard to check for keyboardchanges
glutKeyboardFunc(keyboard);

#initate our window
init_gl_window($s_width, $s_height)
#tells the program to loop
glutMainLoop();




